#include "mpu6050.h"
#include "bsp_iic.h"
#include "delay.h"

#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

void MPU_Write_Byte(uint8_t reg, uint8_t data)
{
    IIC_Write(MPU_ADDR, reg, 1, &data);
}

uint8_t MPU_Read_Byte(uint8_t reg)
{
    uint8_t data;
    IIC_Read(MPU_ADDR, reg, 1, &data);
    return data;
}

void MPU_Read_Len(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
    IIC_Read(addr, reg, len, buf);
}
//初始化MPU6050
//返回值:0,成功
//    其他,错误代码
uint8_t res;
uint8_t MPU_Init(void)
{
    IIC_Init();                               //初始化IIC总线
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X80);  //复位MPU6050
    delay_ms(100);
    MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X00);  //唤醒MPU6050
    MPU_Set_Gyro_Fsr(3);                      //陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0);                     //加速度传感器,±2g
    MPU_Set_Rate(50);                         //设置采样率50Hz
    MPU_Write_Byte(MPU_INT_EN_REG, 0X00);     //关闭所有中断
    MPU_Write_Byte(MPU_USER_CTRL_REG, 0X00);  //I2C主模式关闭
    MPU_Write_Byte(MPU_FIFO_EN_REG, 0X00);    //关闭FIFO
    MPU_Write_Byte(MPU_INTBP_CFG_REG, 0X80);  //INT引脚低电平有效
    res = MPU_Read_Byte(MPU_DEVICE_ID_REG);
    if (res == MPU_ADDR)  //器件ID正确
    {
        MPU_Write_Byte(MPU_PWR_MGMT1_REG, 0X01);  //设置CLKSEL,PLL X轴为参考
        MPU_Write_Byte(MPU_PWR_MGMT2_REG, 0X00);  //加速度与陀螺仪都工作
        MPU_Set_Rate(50);                         //设置采样率为50Hz
    }
    else
        return 1;
    return 0;
}
//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败
void MPU_Set_Gyro_Fsr(uint8_t fsr)
{
    MPU_Write_Byte(MPU_GYRO_CFG_REG, fsr << 3);  //设置陀螺仪满量程范围
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败
void MPU_Set_Accel_Fsr(uint8_t fsr)
{
    MPU_Write_Byte(MPU_ACCEL_CFG_REG, fsr << 3);  //设置加速度传感器满量程范围
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败
void MPU_Set_LPF(uint16_t lpf)
{
    uint8_t data = 0;
    if (lpf >= 188)
        data = 1;
    else if (lpf >= 98)
        data = 2;
    else if (lpf >= 42)
        data = 3;
    else if (lpf >= 20)
        data = 4;
    else if (lpf >= 10)
        data = 5;
    else
        data = 6;
    MPU_Write_Byte(MPU_CFG_REG, data);  //设置数字低通滤波器
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败
void MPU_Set_Rate(uint16_t rate)
{
    uint8_t data;
    if (rate > 1000)
        rate = 1000;
    if (rate < 4)
        rate = 4;
    data = 1000 / rate - 1;
    MPU_Write_Byte(MPU_SAMPLE_RATE_REG, data);  //设置数字低通滤波器
    MPU_Set_LPF(rate / 2);                      //自动设置LPF为采样率的一半
}

//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    uint8_t buf[2];
    short raw;
    float temp;
    MPU_Read_Len(MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf);
    raw = ((uint16_t)buf[0] << 8) | buf[1];
    temp = 36.53 + ((double)raw) / 340;
    return temp * 100;
    ;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
void MPU_Get_Gyro(int16_t gyro[3])
{
    uint8_t buf[6];
    MPU_Read_Len(MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf);
    gyro[0] = ((uint16_t)buf[0] << 8) | buf[1];
    gyro[1] = ((uint16_t)buf[2] << 8) | buf[3];
    gyro[2] = ((uint16_t)buf[4] << 8) | buf[5];
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
void MPU_Get_Acc(int16_t acc[3])
{
    uint8_t buf[6];
    MPU_Read_Len(MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf);
    acc[0] = ((uint16_t)buf[0] << 8) | buf[1];
    acc[1] = ((uint16_t)buf[2] << 8) | buf[3];
    acc[2] = ((uint16_t)buf[4] << 8) | buf[5];
}
